
#include <rclcpp/rclcpp.hpp>
#include <QStringBuilder>
#include <rclcpp/rclcpp.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_thread.h"
#include <rviz_common/logging.hpp>
#include <visualization_msgs/msg/interactive_marker_feedback.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_util.h"


using rviz_custom_plugins::utils::MARKER_ADDED;
using rviz_custom_plugins::utils::MARKER_UPDATED;
using rviz_custom_plugins::utils::MARKER_REMOVED;


namespace rviz_custom_plugins
{

namespace utils
{

InteractiveMarkerThread::InteractiveMarkerThread(QObject *parent) : QThread(parent) {
    isActive = false;
    qRegisterMetaType<interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr>(
       "interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr"
    );
	feedbackFunc = [this](InteractiveMarkerServer::FeedbackConstSharedPtr feedback) {
        handleMarkerFeedback(feedback);
    	emit interactiveMarkerFeedback(feedback);
	};
}


InteractiveMarkerThread::~InteractiveMarkerThread() {

}


void InteractiveMarkerThread::run() {
    QString name = objectName();
    QString nodeName = "node_" % name;
    rosNode = rclcpp::Node::make_shared(nodeName.toStdString());
    server = std::make_shared<InteractiveMarkerServer>(name.toStdString(), rosNode);
    rclcpp::spin(rosNode);
    // rclcpp::shutdown();
}


void InteractiveMarkerThread::insert(const visualization_msgs::msg::InteractiveMarker & marker) {
    QMutexLocker locker(&mutex);
    server->insert(marker, feedbackFunc);
    server->applyChanges();

    bool markerExisted = markerMap.count(marker.name) > 0;
    markerEventFeedback(marker, markerExisted ? MARKER_UPDATED : MARKER_ADDED);
}


bool InteractiveMarkerThread::erase(const std::string & name) {
    QMutexLocker locker(&mutex);
    bool success = server->erase(name);

    if (success) {
        server->applyChanges();

        // 找到对应的marker
        auto it = markerMap.find(name);
        auto markerPtr = it->second;
        markerEventFeedback(*markerPtr, MARKER_REMOVED);
    }
    return success;
}


std::size_t InteractiveMarkerThread::size() const {
    // QMutexLocker locker(&mutex);
    return server->size();
}


bool InteractiveMarkerThread::get(const std::string & name, visualization_msgs::msg::InteractiveMarker & intMarker) const {
    return server->get(name, intMarker);
}


void InteractiveMarkerThread::setActive(bool active) {
    QMutexLocker locker(&mutex);
    isActive = active;
	for (auto it = markerMap.begin(); it != markerMap.end(); ++it) {
		auto markerPtr = it->second;
		if(active) {
			server->insert(*markerPtr, feedbackFunc);
		} else {
			visualization_msgs::msg::InteractiveMarker intMarker = *markerPtr;
			for(auto& control : intMarker.controls) {
				control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE;
			}
			server->insert(intMarker, feedbackFunc);
		}
	}
	server->applyChanges();
    emit activeStatusChanged(active);
}

bool InteractiveMarkerThread::getActive() const {
    return isActive;
}

void InteractiveMarkerThread::handleMarkerFeedback(InteractiveMarkerServer::FeedbackConstSharedPtr feedback)
{
    if(MARKER_ADDED == feedback->client_id || MARKER_UPDATED == feedback->client_id)
    {
        visualization_msgs::msg::InteractiveMarker intMarker;
        server->get(feedback->marker_name, intMarker);
        markerMap[intMarker.name] = std::make_shared<visualization_msgs::msg::InteractiveMarker>(intMarker);
    }
    else if(MARKER_REMOVED == feedback->client_id)
    {
        markerMap.erase(feedback->marker_name);
        return;
    }
	switch (feedback->event_type) {
        case visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE:
        {
            QString markerName = QString::fromStdString(feedback->marker_name);
			auto markerPtr = markerMap[feedback->marker_name];
			markerPtr->pose = feedback->pose;
            break;
        }
    }
}


void InteractiveMarkerThread::markerEventFeedback(const visualization_msgs::msg::InteractiveMarker& intMarker, std::string event)
{
    auto feedback = std::make_shared<visualization_msgs::msg::InteractiveMarkerFeedback>();
    feedback->marker_name = intMarker.name;
    feedback->client_id = event;
    feedback->event_type = visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE;
    feedback->pose = intMarker.pose;
    feedback->header.frame_id = "map";
    feedback->header.stamp = rclcpp::Clock().now();
    feedbackFunc(feedback);
}

}

}
